import threading
import time

import cv2
import numpy as np

# 飞行正前方位置所在图片百分比
center_x = 0.5
center_y = 150 / 940

b_deal1 = 11
b_deal2 = 7
circle_color = (255, 255, 255)

font = cv2.FONT_HERSHEY_SIMPLEX
font_scale = 1.5
font_color = (255, 255, 255)
line_thickness = 2
place = (20, 60)
Text = "Distance:{:.0f}cm"


class Tool:
    img = None
    out_img = None
    b_img = None
    x = -1
    y = -1
    r = -1
    d = -1
    dx = -1
    dy = -1

    high = 720
    width = 960
    cx = width / 2
    cy = high / 2

    cr1 = 100
    cr2 = 75
    min_r = 100
    max_r = 300
    # 相机焦距(像素)
    f = 900
    # 目标圆半径(cm)
    cr = 40

    def __init__(self, qi=None, qb=None, qo=None, qm=None):
        self.new_img_flag = None
        self.qi, self.qb, self.qo, self.qm = qi, qb, qo, qm
        self.deal_thread = threading.Thread(target=self.identify, args=(qb, qo, qm))
        self.show_thread = threading.Thread(target=self.show)
        self.update_thread = threading.Thread(target=self.img_update)

    def img_set(self, img):
        self.high, self.width, _ = img.shape
        if self.high > 700 and self.width > 700:
            self.cx = self.width * center_x
            self.cy = self.high * center_y
            self.cr1 = 100
            self.cr2 = 75
            self.min_r = 100
            self.max_r = 300
            self.f = 900
            self.cr = 40
        # elif self.high > 700 and self.width < 700:
        #     self.img = self.img[0:120, 0:160]
        #     self.cx = 120 / 2
        #     self.cy = self.high / 2
        #     self.cr1 = 100
        #     self.cr2 = 75
        #     self.min_r = 50
        #     self.max_r = 160
        #     self.f = 230
        #     self.cr = 30
        else:
            self.cx = self.width / 2
            self.cy = self.high / 2
            self.cr1 = 100
            self.cr2 = 75
            self.min_r = 30
            self.max_r = 160
            self.f = 230
            self.cr = 30
        self.img = img

    def img_update(self):
        if self.qi is not None:
            while True:
                self.img_set(self.qi.get())
                self.new_img_flag = True

    def start(self):
        self.update_thread.start()
        self.deal_thread.start()

    def start_show(self):
        self.show_thread.start()

    def show(self):
        while True:
            if self.out_img is not None:
                # self.print_result()
                cv2.imshow('out_img', self.out_img)
                # cv2.imshow('b_img', self.b_img)
                cv2.waitKey(1)

    def identify(self, qb, qo, qm):
        img = None
        g_img = None
        b_img = None
        out_img = None
        while True:
            if not self.new_img_flag:
                continue
            img = self.img
            if img is None:
                continue
            g_img = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
            b_img = cv2.adaptiveThreshold(g_img, 255, cv2.ADAPTIVE_THRESH_GAUSSIAN_C, cv2.THRESH_BINARY, b_deal1,
                                          b_deal2)  # 自动阈值二值化
            self.b_img = b_img
            circles = cv2.HoughCircles(b_img, cv2.HOUGH_GRADIENT, 1, 170, param1=self.cr1, param2=self.cr2,
                                       minRadius=self.min_r,
                                       maxRadius=self.max_r)
            if circles is not None:
                circles = np.round(circles[0, :]).astype("int")
                (self.x, self.y, self.r) = max(circles, key=lambda rd: rd[2])
                self.d = int(self.cr / self.r * self.f)
                mx = self.x - self.cx
                self.dx = int(mx / self.f * self.d)
                my = self.y - self.cy
                self.dy = int(my / self.f * self.d)
                if qm is not None:
                    qm.put((self.dx, self.dy, self.d))
            out_img = g_img
            if self.x >= 0 and self.y >= 0 and self.r >= 0:
                cv2.putText(out_img, Text.format(self.d), place, font, font_scale, font_color,
                            line_thickness, cv2.LINE_AA)
                cv2.circle(out_img, (self.x, self.y), self.r, circle_color, 4)
            if qb is not None:
                qb.put(b_img)
            if qo is not None:
                qo.put(out_img)
            self.out_img = out_img
            self.new_img_flag = False

    def print_result(self):
        # if self.x == -1:
        #     print("Not found")
        # else:
        print("Has found")
        print("img message:")
        print("  high:{}px width:{}px rgb_mod:{}".format(self.img.shape[0], self.img.shape[1], self.img.shape[2]))
        print("circle center position:")
        print("  x:{}px  y:{}px   r:{}px".format(self.x, self.y, self.r))
        print("The target point that the plane needs to go to:")
        print("  x:{}px  y:{}px".format(self.cx, self.cy))
        print("move :")
        print("  dx:{}cm   dy:{}cm".format(self.dx, self.dy))


if __name__ == '__main__':
    tool = Tool()
    tool.img_set(cv2.imread('1.png'))

    tool.start()
    time.sleep(1)
    tool.print_result()
    cv2.imshow('show', tool.out_img)
    cv2.waitKey(0)
